#include <stdio.h>
#include <string.h>
#include "car.h"

/***********************

				car wheel map 

				***     ***
				*0*-----*1*
				***  |  ***
						 |
						 |
				***  |  ***
				*2*-----*3*
				***     ***

**************************/
int car_move(car_type *pCar, e_car_direction direction)
{
	int ret = 0;
	pCar->car_direction = direction;
	switch(direction) {
		case car_stop: {
			wheel_set_scroll(&pCar->car_wheels[0], wheel_stop);
			wheel_set_scroll(&pCar->car_wheels[1], wheel_stop);
			wheel_set_scroll(&pCar->car_wheels[2], wheel_stop);
			wheel_set_scroll(&pCar->car_wheels[3], wheel_stop);
			break;
		}
		case car_forward: {
			wheel_set_scroll(&pCar->car_wheels[0], wheel_go);
			wheel_set_scroll(&pCar->car_wheels[1], wheel_go);
			wheel_set_scroll(&pCar->car_wheels[2], wheel_go);
			wheel_set_scroll(&pCar->car_wheels[3], wheel_go);
			break;
		}
		case car_back: {
			wheel_set_scroll(&pCar->car_wheels[0], wheel_back);
			wheel_set_scroll(&pCar->car_wheels[1], wheel_back);
			wheel_set_scroll(&pCar->car_wheels[2], wheel_back);
			wheel_set_scroll(&pCar->car_wheels[3], wheel_back);
			break;
		}
		case car_left: {
			wheel_set_scroll(&pCar->car_wheels[0], wheel_back);
			wheel_set_scroll(&pCar->car_wheels[1], wheel_go);
			wheel_set_scroll(&pCar->car_wheels[2], wheel_go);
			wheel_set_scroll(&pCar->car_wheels[3], wheel_back);
			break;
		}
		case car_right: {
			wheel_set_scroll(&pCar->car_wheels[0], wheel_go);
			wheel_set_scroll(&pCar->car_wheels[1], wheel_back);
			wheel_set_scroll(&pCar->car_wheels[2], wheel_back);
			wheel_set_scroll(&pCar->car_wheels[3], wheel_go);
			break;
		}
		case car_forward_left: {
			wheel_set_scroll(&pCar->car_wheels[0], wheel_stop);
			wheel_set_scroll(&pCar->car_wheels[1], wheel_go);
			wheel_set_scroll(&pCar->car_wheels[2], wheel_go);
			wheel_set_scroll(&pCar->car_wheels[3], wheel_stop);
			break;
		}
		case car_forward_right: {
			wheel_set_scroll(&pCar->car_wheels[0], wheel_go);
			wheel_set_scroll(&pCar->car_wheels[1], wheel_stop);
			wheel_set_scroll(&pCar->car_wheels[2], wheel_stop);
			wheel_set_scroll(&pCar->car_wheels[3], wheel_go);
			break;
		}
		case car_back_left: {
			wheel_set_scroll(&pCar->car_wheels[0], wheel_back);
			wheel_set_scroll(&pCar->car_wheels[1], wheel_stop);
			wheel_set_scroll(&pCar->car_wheels[2], wheel_stop);
			wheel_set_scroll(&pCar->car_wheels[3], wheel_back);
			break;
		}
		case car_back_right: {
			wheel_set_scroll(&pCar->car_wheels[0], wheel_stop);
			wheel_set_scroll(&pCar->car_wheels[1], wheel_back);
			wheel_set_scroll(&pCar->car_wheels[2], wheel_back);
			wheel_set_scroll(&pCar->car_wheels[3], wheel_stop);
			break;
		}
		case car_turn_left: {
			wheel_set_scroll(&pCar->car_wheels[0], wheel_back);
			wheel_set_scroll(&pCar->car_wheels[1], wheel_go);
			wheel_set_scroll(&pCar->car_wheels[2], wheel_back);
			wheel_set_scroll(&pCar->car_wheels[3], wheel_go);
			break;
		}
		case car_turn_right: {
			wheel_set_scroll(&pCar->car_wheels[0], wheel_go);
			wheel_set_scroll(&pCar->car_wheels[1], wheel_back);
			wheel_set_scroll(&pCar->car_wheels[2], wheel_go);
			wheel_set_scroll(&pCar->car_wheels[3], wheel_back);
			break;
		}
		default: {
			wheel_set_scroll(&pCar->car_wheels[0], wheel_stop);
			wheel_set_scroll(&pCar->car_wheels[1], wheel_stop);
			wheel_set_scroll(&pCar->car_wheels[2], wheel_stop);
			wheel_set_scroll(&pCar->car_wheels[3], wheel_stop);
			break;
		}
	}
	return ret;
}

int car_init(car_type *pCar)
{
	int ret = -1;
	int i,j;
	car_type car;
	if(pCar == NULL)
	{
		printf("pCar is NULL");
		return ret;
	}
	memset(&car, 0x00, sizeof(car_type));
	//wheel init GPIO
	car.car_wheels[0].ios[0] = GPIO_00;
	car.car_wheels[0].ios[1] = GPIO_01;
	car.car_wheels[1].ios[0] = GPIO_02;
	car.car_wheels[1].ios[1] = GPIO_03;
	car.car_wheels[2].ios[0] = GPIO_04;
	car.car_wheels[2].ios[1] = GPIO_05;
	car.car_wheels[3].ios[0] = GPIO_06;
	car.car_wheels[3].ios[1] = GPIO_07;
	for(i=0;i<4;i++)
	{
		for(j=0;j<2;j++)
		{
			gpio_init(car.car_wheels[i].ios[j]);
		}
	}
	memcpy(pCar, &car, sizeof(car_type));
	ret = 0;
	
	return ret;
}
